#include "Vehicle.h"
#include "KeyManager.h"

Vehicle::Vehicle(VehicleDescription& desc, btDynamicsWorld* simWorld, std::vector<btRigidBody *>& bodies)
{
	m_caster = new btDefaultVehicleRaycaster(simWorld);

	btTransform t;
	t.setIdentity();
	t.setOrigin(desc.mVehicleInitialPosition);

	btDefaultMotionState* motionState = new btDefaultMotionState(t);

	btVector3 boxSize = btVector3(
		desc.mVehicleBodySize.x()/2.f,
		desc.mVehicleBodySize.y()/2.f,
		desc.mVehicleBodySize.z()/2.f
		);

	btBoxShape* boxShape = new btBoxShape(boxSize);
	btVector3 inertia(0,0,0);

	boxShape->calculateLocalInertia(desc.mVehicleMass, inertia);
	btRigidBody::btRigidBodyConstructionInfo bodyConstructionInfo(
		desc.mVehicleMass,
		motionState,
		boxShape,
		inertia
		);

	btRigidBody* vehicleBody = new btRigidBody(bodyConstructionInfo);
	vehicleBody->setActivationState(DISABLE_DEACTIVATION);
	simWorld->addRigidBody(vehicleBody);
	bodies.push_back(vehicleBody);

	m_vehicle = new btRaycastVehicle(m_tuning, vehicleBody, m_caster);

	bool isFrontWheel = true;
	m_vehicle->addWheel(desc.mFrontLeftWheelOffset, desc.mWheelDirection, desc.mWheelAxle,
		desc.mSuspensionRestLength, desc.mWheelRadius, m_tuning, isFrontWheel);
	m_vehicle->addWheel(desc.mFrontRightWheelOffset, desc.mWheelDirection, desc.mWheelAxle,
		desc.mSuspensionRestLength, desc.mWheelRadius, m_tuning, isFrontWheel);

	isFrontWheel = false;
	m_vehicle->addWheel(desc.mRearLeftWheelOffset, desc.mWheelDirection, desc.mWheelAxle,
		desc.mSuspensionRestLength, desc.mWheelRadius, m_tuning, isFrontWheel);
	m_vehicle->addWheel(desc.mRearRightWheelOffset, desc.mWheelDirection, desc.mWheelAxle,
		desc.mSuspensionRestLength, desc.mWheelRadius, m_tuning, isFrontWheel);

	simWorld->addVehicle(m_vehicle);
	m_vehicle->setCoordinateSystem(
		desc.mVehicleCoordinateSystem.x(), 
		desc.mVehicleCoordinateSystem.y(),
		desc.mVehicleCoordinateSystem.z()
		);

	for (int i = 0; i < m_vehicle->getNumWheels(); ++i)
	{
		btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
		wheel.m_suspensionStiffness = desc.mSuspensionStiffness;
		wheel.m_wheelsDampingRelaxation = desc.mWheelsDampingRelaxation;
		wheel.m_wheelsDampingCompression = desc.mWheelsDampingCompression;
		wheel.m_frictionSlip = desc.mWheelFriction;
		wheel.m_rollInfluence = desc.mRollInfluence;
	}

	m_engineForce = 0.0f;
	m_breakingForce = 0.0f;
	m_steeringAngle = 0.0f;

	m_maxEngineForce = desc.mMaxEngineForce;
	m_maxBreakingForce = desc.mMaxBreakingForce;
	m_maxSteeringAngle = desc.mMaxSteeringAngle;
	m_currentSteeringDirection = STEER_NONE;

	m_gasPadelPressed = false;
	m_breakPadelPressed = false;
}

Vehicle::~Vehicle()
{
}

void Vehicle::pressGasPadel()
{
	m_gasPadelPressed = true;
}

void Vehicle::releaseGasPadel()
{
	m_gasPadelPressed = false;
}

void Vehicle::pressBreakPadel()
{
	m_breakPadelPressed = true;
}

void Vehicle::releaseBreakPadel()
{
	m_breakPadelPressed = false;
}

void Vehicle::setSteerDirection(SteeringDirection direction)
{
	m_currentSteeringDirection = direction;
}

void Vehicle::drive()
{
	//if (m_gasPadelPressed && !m_breakPadelPressed) {
	if (KeyManager::get()->isKeyDown(KEY_W) && !KeyManager::get()->isKeyDown(KEY_S)) {
		float d = (m_maxEngineForce - m_engineForce) * 0.8;
		m_engineForce += d;
		m_breakingForce = 0.0f;
	//} else if (m_breakPadelPressed && !m_gasPadelPressed) {
	} else if (!KeyManager::get()->isKeyDown(KEY_W) && KeyManager::get()->isKeyDown(KEY_S)) {
		float d = (m_maxBreakingForce - m_breakingForce) * 0.8;
		m_breakingForce += d;
		m_engineForce = 0.0f;
	//} else if (m_gasPadelPressed && m_breakPadelPressed) {
	} else if (KeyManager::get()->isKeyDown(KEY_W) && KeyManager::get()->isKeyDown(KEY_S)) {
		// what ?
	} else {
		m_engineForce = 0.0f;
		m_breakingForce = 0.0f;
	}

	/*switch(m_currentSteeringDirection) {
		case STEER_LEFT:
			{
				float d = (m_maxSteeringAngle - m_steeringAngle) * 0.8;
				m_steeringAngle += d;
			}
			break;
		case STEER_RIGHT:
			{
				float d = (m_maxSteeringAngle + m_steeringAngle) * 0.8;
				m_steeringAngle -= d;
			}
			break;
		case STEER_NONE:
			{
				if (m_steeringAngle < 0) {
					float d = -m_steeringAngle * 0.5;
					m_steeringAngle += d;
				} else {
					float d = m_steeringAngle * 0.5;
					m_steeringAngle -= d;
				}
			}
			break;
		default:
			break;
	}*/
	if (KeyManager::get()->isKeyDown(KEY_A) && !KeyManager::get()->isKeyDown(KEY_D)) {
		float d = (m_maxSteeringAngle - m_steeringAngle) * 0.8;
		m_steeringAngle += d;
	} else if (!KeyManager::get()->isKeyDown(KEY_A) && KeyManager::get()->isKeyDown(KEY_D)) {
		float d = (m_maxSteeringAngle + m_steeringAngle) * 0.8;
		m_steeringAngle -= d;
	} else if (KeyManager::get()->isKeyDown(KEY_A) && KeyManager::get()->isKeyDown(KEY_D)) {
		// what ?
	} else {
		if (m_steeringAngle < 0) {
			float d = -m_steeringAngle * 0.5;
			m_steeringAngle += d;
		} else {
			float d = m_steeringAngle * 0.5;
			m_steeringAngle -= d;
		}
	}

	m_vehicle->applyEngineForce(m_engineForce, REAR_L);
	m_vehicle->applyEngineForce(m_engineForce, REAR_R);
	m_vehicle->setBrake(m_breakingForce, REAR_L);
	m_vehicle->setBrake(m_breakingForce, REAR_R);
	m_vehicle->setSteeringValue(m_steeringAngle, FRONT_L);
	m_vehicle->setSteeringValue(m_steeringAngle, FRONT_R);
}

int Vehicle::getWheelCount()
{
	return m_vehicle->getNumWheels();
}

float Vehicle::getWheelMatrix(float* mat, int i)
{
	m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(mat);
	return m_vehicle->getWheelInfo(i).m_wheelsRadius;
}